// Have include guard.
#ifndef XBOXDIVESERVICE_H
#define XBOXDIVESERVICE_H

// Include files.
#include <vector>
#include "WPILib.h"
#include "../Inputs/XboxJoystick.hpp"
#include "../Values/XboxJoystickValue.hpp"
#include "../Filters/CompoundFilter.hpp"
#include "Service.hpp"

template
<
	typename Drive,
	typename Value,
	void(*Handler)(Drive &drive, const Value &value)
>
class XboxDriveService:
	public Service
{
	public:
		typedef TwoJoystickValue Input;
		typedef Value Output;
		typedef Filter<Input, Output> Filter;
	protected:
		Drive &m_drive;
		CompoundFilter<Input, Output> m_filter;
		XboxJoystick &m_joystick;
	public:
		XboxDriveService(Drive &drive, boost::shared_ptr<Filter> driveMode, XboxJoystick &joystick) :
			m_drive(RequireControl(drive)),
			m_filter(driveMode),
			m_joystick(joystick)
		{
		}
		
		XboxDriveService(Drive &drive, Filter *driveMode, XboxJoystick &joystick, bool canDeleteDriveMode = false) :
			m_drive(RequireControl(drive)),
			m_filter(driveMode, canDeleteDriveMode),
			m_joystick(joystick)
		{
		}
		
		template<typename T>
		void AddFilter(T *filter, bool canDeleteFilter)
		{
			m_filter.AddFilter(filter, canDeleteFilter);
		}
		
		template<typename T>
		void AddFilter(T filter)
		{
			m_filter.AddFilter(filter);
		}
	protected:
		virtual void HandlePeriodic()
		{
			Input input(m_joystick);
			Output output = m_filter.Apply(input);
			Handler(m_drive, output);
		}
		
		virtual void HandleStop()
		{
			m_drive.Stop();
		}
};

#endif // #ifndef XBOXDIVESERVICE_H
